#! usr/bin/env python
import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
#订阅方实现：订阅消息，传入被转换的坐标点，调用转换算法   
#注意这里完成的是从激光测得物体位置得到物体与小车主体的位置,接收雷达数据，输出小车数据
#导入包
#初始化
#创建订阅对象
#组织被转换的坐标点
#转换逻辑
#输出结果
if __name__ == "__main__":
    rospy.init_node("static_sub_p")
    #创建缓存对象
    buffer = tf2_ros.Buffer()
    #创建订阅对象
    sub = tf2_ros.TransformListener(buffer)
    #组织被转换的坐标点
    ps = tf2_geometry_msgs.PointStamped()
    ps.header.stamp = rospy.Time.now()
    ps.header.frame_id = "laser"
    ps.point.x = 2.0
    ps.point.y = 3.0
    ps.point.z = 5.0
    #转换逻辑实现
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        #转换,此时数据已经传入了buffer，buffer提供转换的函数
        try:
            ps_out = buffer.transform(ps,"base_link")
            rospy.loginfo("转换后的结果：（%.2f,%.2f,%.2f）",ps_out.point.x,
            ps_out.point.y,ps_out.point.z)
        except Exception as e:
            print("未订阅到信息")
        
        rate.sleep()
